! control laws for dust auto-generation codes

! 引用依赖文件
include "mod_param.f90"
include "rcr_vind.f90"
include "flap4rcr.f90"

program cl4dust
    use mod_param 
    implicit none

    real :: out(4,3), outori(4,3)

    ! === mod_param  ===
    print *, "Please input the air velocity: "
    read(*,*) vair 
    print *, "The Flight Speed is: ", vair, "m/s"
    ! vair = 20.0

    T = 5500

    Omega = 35.0
    R = 5.49
    chord = 0.365
    sigma = 0.0635
    Arot = pi*R**2
    cl_a = 6.0
    as = -3.0/180*pi 

    vtip = Omega*R 
    CT = T/(0.5*rho*vtip**2*Arot)
    mu = vair*cos(as)/vtip
    lam0 = vair*sin(as)/vtip

    ! dth = 10.0/180*pi 
    eta0 = 7.5/180*pi 
    thtw = -10.0/180*pi
    ! th75 = eta0+0.75*thtw
    ! thtot = th75+th(1)

    m_ = 8.114
    Ms = 0.5*m_*R**2.0
    Ib = 1.0/3*m_*R**3.0
    lockb = 5.41
    omegan = 1.4
    e = 0.25 

    call sol_cl(outori, out)

    100 format(A40,F18.8,F8.4,F8.4)

    write(*,fmt=100) "Real pitch value for upper rotor:",outori(1,1),outori(1,2),outori(1,3)
    write(*,fmt=100) "Real pitch value for lower rotor:",outori(2,1),outori(2,2),outori(2,3)
    write(*,fmt=100) "Real flap value for upper rotor:", outori(3,1),outori(3,2),outori(3,3)
    write(*,fmt=100) "Real flap value for lower rotor:", outori(4,1),outori(4,2),outori(4,3)
    write(*,*)
    write(*,fmt=100) "Pitch value for upper rotor:",out(1,1),out(1,2),out(1,3)
    write(*,fmt=100) "Pitch value for lower rotor:",out(2,1),out(2,2),out(2,3)
    write(*,fmt=100) "Flap value for upper rotor:", out(3,1),out(3,2),out(3,3)
    write(*,fmt=100) "Flap value for lower rotor:", out(4,1),out(4,2),out(4,3)
    ! read(*,*)

end program cl4dust

subroutine controllaws(id, out)
    use mod_param, only: vair
    implicit none 

    real :: inp
    character(8) :: id 
    real :: out

    inp = vair

    select case(id)
    case("col_0000")
        out = -0.00014649*inp**3+0.01509459*inp**2-0.43410677*inp+11.58751546
    case("col_diff")
        out = 0.00000942*inp**3 - 0.00117038*inp**2 + 0.04266543*inp- 0.41398092 
    case("lon_0000")
        out = -(0.00000362*inp**4 - 0.00033083*inp**3 + 0.01084908*inp**2 - 0.23758288*inp - 0.01839867)
    case("lat_0000")
        out = -(0.00000414*inp**3 + 0.00017013*inp**2 - 0.01981304*inp + 0.02154562)
    case("lat_diff")
        out = -(0.00404217*inp**2 + 0.04696362*inp + 0.75970719)
    case("tai_0000")
        out = 0.00040753*inp**3 - 0.02176337*inp**2 + 1.09173059*inp + 13.66563331 
    case default 
        print *, "The id inputed is invalid!"
    end select 

    return 
end subroutine controllaws

subroutine sol_cl(outori, out)
    use mod_param, only: pi 
    implicit none 

    ! real :: inp ! 输入
    real :: outori(4,3), out(4,3) ! 输出
    real :: claw, &
            cltmp(1:6), clu(1:3), cll(1:3), &
            clu4dust(1:3), cll4dust(1:3), &
            betau(1:3), betal(1:3), &
            bu4dust(1:3), bl4dust(1:3) ! 中间量
    character(8) :: id(1:6) ! 中间量
    integer :: i ! 循环参数

    id = (/"col_0000", "col_diff","lat_0000","lat_diff","lon_0000","tai_0000"/)

    do i=1,6
        call controllaws(id(i), claw)
        cltmp(i) = claw
    end do
    ! print *, cltmp

    clu = (/(cltmp(1)+cltmp(2)), (cltmp(3)+cltmp(4)), cltmp(5)/)
    cll = (/(cltmp(1)-cltmp(2)), (cltmp(3)-cltmp(4)), cltmp(5)/)

    clu4dust = (/clu(1), sign(1.0,-clu(2))*sqrt(clu(2)**2+clu(3)**2), atan(clu(3)/clu(2))/)
    cll4dust = (/cll(1), sign(1.0,-cll(2))*sqrt(cll(2)**2+cll(3)**2), atan(cll(3)/cll(2))/)

    call sol_beta(clu*(pi/180), betau)
    call sol_bint(clu*(pi/180), cll*(pi/180), betal)
    ! print *, betau
    ! print *, betal 

    bu4dust = (/betau(1), sign(1.0,-betau(2))*sqrt(betau(2)**2+betau(3)**2), atan(betau(3)/betau(2))/)*(180/pi)
    bl4dust = (/betal(1), sign(1.0,-betal(2))*sqrt(betal(2)**2+betal(3)**2), atan(betal(3)/betal(2))/)*(180/pi)


    do i=1,3
        outori(1,i) = clu(i)
        outori(2,i) = cll(i)
        outori(3,i) = betau(i)*(180/pi)
        outori(4,i) = betal(i)*(180/pi)
    end do 

    do i=1,3
        out(1,i) = -1.0*clu4dust(i)
        out(2,i) = cll4dust(i)
        out(3,i) = bu4dust(i)
        out(4,i) = bl4dust(i)
    end do 

    return 
end subroutine sol_cl